/************************************************************************************************************************
 * 插补器测试
 * **********************************************************************************************************************/
#include <gtest/gtest.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <thread>
#include <vector>
#include "axis/planner.h"

using namespace axis;

#define TRAJ_INTERVAL 0.008
#define PLAN_INTERVAL 0.001

class PLANNERTEST : public ::testing::Test
{
protected:
    void SetUp() override
    {
        planner_.init_traj_planner(PLAN_INTERVAL, TRAJ_INTERVAL);
        for (int i = 0; i < DO_TIMES; i++)
        {
            TrajPoint a = {};
            a.is_real = true;
            a.pos = i * 10;
            a.vel = i * 1000;
            point_list_.push_back(a);  // 启动点，第一次push后，才是真正的插补，此时，直接以该点作为目标点；
        }
    }
    Planner planner_;
    const int DO_TIMES = 1000;
    const int TRAJ_RATIO = 8;
    std::vector<TrajPoint> point_list_;
};

TEST_F(PLANNERTEST, PLAN_TYPE)
{
    srand(time(0));
    double s0 = (-RAND_MAX + rand() * 2.0) / (double)RAND_MAX * M_PI * 10;
    double se = (-RAND_MAX + rand() * 2.0) / (double)RAND_MAX * M_PI * 10;
    double v0 = (-RAND_MAX + rand() * 2.0) / (double)RAND_MAX * M_PI * 10;
    double ve = (-RAND_MAX + rand() * 2.0) / (double)RAND_MAX * M_PI * 10;
    double a0 = (-RAND_MAX + rand() * 2.0) / (double)RAND_MAX * M_PI * 10;
    double ae = (-RAND_MAX + rand() * 2.0) / (double)RAND_MAX * M_PI * 10;
    double j0 = (-RAND_MAX + rand() * 2.0) / (double)RAND_MAX * M_PI * 10;
    double je = (-RAND_MAX + rand() * 2.0) / (double)RAND_MAX * M_PI * 10;

    double pos = 0;
    // linear plan
    planner_.do_plan(Planner::INTERP_LINEAR, s0, se, v0, ve, a0, ae, j0, je, TRAJ_INTERVAL);
    planner_.calc_s(0, pos);
    ASSERT_NEAR(pos, s0, 1e-8);
    planner_.calc_s(TRAJ_INTERVAL, pos);
    ASSERT_NEAR(pos, se, 1e-8);

    // 3th plan
    planner_.do_plan(Planner::INTERP_POLY3, s0, se, v0, ve, a0, ae, j0, je, TRAJ_INTERVAL);
    planner_.calc_s(0, pos);
    ASSERT_NEAR(pos, s0, 1e-8);
    planner_.calc_s(TRAJ_INTERVAL, pos);
    ASSERT_NEAR(pos, se, 1e-8);

    // 5th plan
    planner_.do_plan(Planner::INTERP_POLY5, s0, se, v0, ve, a0, ae, j0, je, TRAJ_INTERVAL);
    planner_.calc_s(0, pos);
    ASSERT_NEAR(pos, s0, 1e-8);
    planner_.calc_s(TRAJ_INTERVAL, pos);
    ASSERT_NEAR(pos, se, 1e-8);

    // 7th plan
    planner_.do_plan(Planner::INTERP_POLY7, s0, se, v0, ve, a0, ae, j0, je, TRAJ_INTERVAL);
    planner_.calc_s(0, pos);
    ASSERT_NEAR(pos, s0, 1e-8);
    planner_.calc_s(TRAJ_INTERVAL, pos);
    ASSERT_NEAR(pos, se, 1e-8);  // 7次多项式计算，可能造成一位误差
}

TEST_F(PLANNERTEST, PREDICT_TYPE)
{
    srand(time(0));
    double s0 = (-RAND_MAX + rand() * 2.0) / (double)RAND_MAX * M_PI * 10;
    double v0 = (-RAND_MAX + rand() * 2.0) / (double)RAND_MAX * M_PI * 10;
    double a0 = (-RAND_MAX + rand() * 2.0) / (double)RAND_MAX * M_PI * 10;
    double j0 = (-RAND_MAX + rand() * 2.0) / (double)RAND_MAX * M_PI * 10;
    TrajPoint cur = {s0, v0, a0, j0};
    TrajPoint pre = {};

    // hold vel
    pre = planner_.do_predict(Planner::PREDICT_HOLD_VEL, cur);
    EXPECT_NEAR(pre.pos, (s0 + v0 * TRAJ_INTERVAL), 1e-6);
    EXPECT_TRUE(fabs(pre.vel - v0) < 1e-6);
    EXPECT_TRUE(fabs(pre.acc - 0) < 1e-6);
    EXPECT_TRUE(fabs(pre.jerk - 0) < 1e-6);

    // hold acc
    pre = planner_.do_predict(Planner::PREDICT_HOLD_ACC, cur);
    EXPECT_NEAR(pre.pos, (s0 + (v0 + a0 * TRAJ_INTERVAL) * TRAJ_INTERVAL), 1e-6);
    EXPECT_TRUE(fabs(pre.vel - (v0 + a0 * TRAJ_INTERVAL)) < 1e-6);
    EXPECT_TRUE(fabs(pre.acc - a0) < 1e-6);
    EXPECT_TRUE(fabs(pre.jerk - 0) < 1e-6);

    // hold jerk
    pre = planner_.do_predict(Planner::PREDICT_HOLD_JERK, cur);
    EXPECT_NEAR(pre.pos, (s0 + (v0 + (a0 + j0 * TRAJ_INTERVAL) * TRAJ_INTERVAL) * TRAJ_INTERVAL), 1e-6);
    EXPECT_TRUE(fabs(pre.vel - (v0 + (a0 + j0 * TRAJ_INTERVAL) * TRAJ_INTERVAL)) < 1e-6);
    EXPECT_TRUE(fabs(pre.acc - (a0 + j0 * TRAJ_INTERVAL)) < 1e-6);
    EXPECT_TRUE(fabs(pre.jerk - j0) < 1e-6);
}

TEST_F(PLANNERTEST, DOPLAN_LINEAR)
{
    srand(time(0));

    ASSERT_EQ(planner_.do_interp(Planner::INTERP_LINEAR, Planner::PREDICT_HOLD_VEL), 0);  //初始点插补

    TrajPoint a = {};
    a.is_real = true;
    a.pos = (-RAND_MAX + rand() * 2.0) / (double)RAND_MAX * M_PI * 10;
    a.vel = (-RAND_MAX + rand() * 2.0) / (double)RAND_MAX * M_PI * 10;
    planner_.push_pos(a);  // 启动点，第一次push后，才是真正的插补，此时，直接以该点作为目标点；

    ASSERT_EQ(planner_.do_interp(Planner::INTERP_LINEAR, Planner::PREDICT_HOLD_VEL), 0);  // 真插补
    double pre_pos = 0;
    double interp_pos = 0;
    // 验证插补点
    for (int i = 1; i <= (TRAJ_INTERVAL / PLAN_INTERVAL); i++)
    {
        planner_.calc_s(i * PLAN_INTERVAL, interp_pos);
        pre_pos = a.pos / TRAJ_INTERVAL * PLAN_INTERVAL * i;  // 初始的该点，由0插到目标值
        EXPECT_NEAR(interp_pos, pre_pos, 1e-6);
    }
    ASSERT_NEAR(a.pos, pre_pos, 1e-8);  // 插补终点一定时预测终点
    ASSERT_EQ(planner_.get_planner_target().is_planned, true);

    ASSERT_EQ(planner_.do_interp(Planner::INTERP_LINEAR, Planner::PREDICT_HOLD_VEL), 0);  // 预测插补
    // 验证预测点
    TrajPoint a_pre = planner_.get_planner_target();
    EXPECT_EQ(a_pre.is_real, false);
    double pos_pre = a.pos + a.vel * TRAJ_INTERVAL;
    EXPECT_EQ(a_pre.pos, pos_pre);
    EXPECT_EQ(a_pre.vel, a.vel);
    EXPECT_EQ(a_pre.acc, 0);
    EXPECT_EQ(a_pre.jerk, 0);
    int lost_count = planner_.get_traj_lost_count();
    EXPECT_EQ(lost_count, 1);

    // 验证插补点
    for (int i = 1; i <= (TRAJ_INTERVAL / PLAN_INTERVAL); i++)
    {
        planner_.calc_s(i * PLAN_INTERVAL, interp_pos);
        pre_pos = a.pos + a.vel * PLAN_INTERVAL * i;  // 预测点是用速度计算出来的
        EXPECT_NEAR(interp_pos, pre_pos, 1e-6);
    }
    ASSERT_NEAR(a_pre.pos, pre_pos, 1e-8);  // 插补终点一定时预测终点

    TrajPoint b = {a.pos + 1000};  // 实际插补点到达,这个点到达时，是不会触发插补的
    b.is_real = true;
    planner_.push_pos(b);
    EXPECT_EQ(planner_.get_planner_target().is_real, true);
    double err = b.pos - a_pre.pos;
    EXPECT_EQ(planner_.get_planner_target().err, err);

    TrajPoint c = {b.pos + 5000, 200};  // 实际插补点到达，这时候触发了插补
    c.is_real = true;
    planner_.push_pos(c);

    ASSERT_EQ(planner_.do_interp(Planner::INTERP_LINEAR, Planner::PREDICT_HOLD_VEL), 0);  //单点预测插补
    // 验证插补点
    for (int i = 1; i <= (TRAJ_INTERVAL / PLAN_INTERVAL); i++)
    {
        planner_.calc_s(i * PLAN_INTERVAL, interp_pos);
        pre_pos = (double)(c.pos - (b.pos - err)) / TRAJ_INTERVAL * (PLAN_INTERVAL * i) + b.pos - err;
        EXPECT_NEAR(interp_pos, pre_pos, 1e-6);
    }
    ASSERT_NEAR(c.pos, interp_pos, 1e-8);  // 插补终点一定时预测终点

    lost_count = planner_.get_traj_lost_count();
    EXPECT_EQ(lost_count, 0);
}

TEST_F(PLANNERTEST, DOPLAN_LINEAR_ALOT)
{
    double pre_pos = 0;
    double interp_pos = 0;
    srand(time(0));

    TrajPoint a = {};
    a.is_real = true;
    a.pos = (-RAND_MAX + rand() * 2.0) / (double)RAND_MAX * M_PI * 10;
    a.vel = (-RAND_MAX + rand() * 2.0) / (double)RAND_MAX * M_PI * 10;
    planner_.push_pos(a);  // 启动点，第一次push后，才是真正的插补，此时，直接以该点作为目标点；

    ASSERT_EQ(planner_.do_interp(Planner::INTERP_LINEAR, Planner::PREDICT_HOLD_VEL), 0);  // 真插补

    for (int try_time = 0; try_time < 10; try_time++)
    {
        TrajPoint a = planner_.get_planner_target();
        ASSERT_EQ(planner_.do_interp(Planner::INTERP_LINEAR, Planner::PREDICT_HOLD_VEL), 0);  // 预测插补
        // 验证预测点
        TrajPoint a_pre = planner_.get_planner_target();
        EXPECT_EQ(a_pre.is_real, false);
        double pos_pre = a.pos + a.vel * TRAJ_INTERVAL;
        EXPECT_NEAR(a_pre.pos, pos_pre, 1e-6);
        EXPECT_NEAR(a_pre.vel, a.vel, 1e-8);
        EXPECT_EQ(a_pre.acc, 0);
        EXPECT_EQ(a_pre.jerk, 0);
        int lost_count = planner_.get_traj_lost_count();
        EXPECT_EQ(lost_count, 1);

        // 验证插补点
        for (int i = 1; i <= (TRAJ_INTERVAL / PLAN_INTERVAL); i++)
        {
            planner_.calc_s(i * PLAN_INTERVAL, interp_pos);
            pre_pos = a.pos + a.vel * PLAN_INTERVAL * i;  // 预测点是用速度计算出来的
            EXPECT_NEAR(interp_pos, pre_pos, 1e-6);
        }
        ASSERT_NEAR(a_pre.pos, pre_pos, 1e-8);  // 插补终点一定时预测终点

        TrajPoint b = {a.pos + 1000};  // 实际插补点到达,这个点到达时，是不会触发插补的
        b.is_real = true;
        planner_.push_pos(b);
        EXPECT_EQ(planner_.get_planner_target().is_real, true);
        double err = b.pos - a_pre.pos;
        EXPECT_NEAR(planner_.get_planner_target().err, err, 1e-8);

        TrajPoint c = {b.pos + 5000, 200};  // 实际插补点到达，这时候触发了插补
        c.is_real = true;
        planner_.push_pos(c);

        ASSERT_EQ(planner_.do_interp(Planner::INTERP_LINEAR, Planner::PREDICT_HOLD_VEL), 0);  //单点预测插补
        // 验证插补点
        for (int i = 1; i <= (TRAJ_INTERVAL / PLAN_INTERVAL); i++)
        {
            planner_.calc_s(i * PLAN_INTERVAL, interp_pos);
            pre_pos = (double)(c.pos - (b.pos - err)) / TRAJ_INTERVAL * (PLAN_INTERVAL * i) + b.pos - err;
            EXPECT_NEAR(interp_pos, pre_pos, 1e-6);
        }
        ASSERT_NEAR(c.pos, interp_pos, 1e-8);  // 插补终点一定时预测终点

        lost_count = planner_.get_traj_lost_count();
        EXPECT_EQ(lost_count, 0);
    }
}

#include <pthread.h>
static pthread_cond_t rtbus_task_cond = PTHREAD_COND_INITIALIZER;
static pthread_mutex_t rtbus_task_mutex = PTHREAD_MUTEX_INITIALIZER;
static int rtbus_ready = 0;
static int exit_flag = 0;

void rtbus_schedule_motion()
{
    pthread_mutex_lock(&rtbus_task_mutex);
    //TODO:准备反馈数据
    rtbus_ready = 1;
    pthread_cond_broadcast(&rtbus_task_cond);  //触发traj_task运行
    pthread_mutex_unlock(&rtbus_task_mutex);
    // zlog()->debug("[RtBus] Send motion task signal");
}

void rtbus_schedule_wait()
{
    pthread_mutex_lock(&rtbus_task_mutex);
    while (!rtbus_ready) { pthread_cond_wait(&rtbus_task_cond, &rtbus_task_mutex); }  // rtbus_ready防止被虚假唤醒
    rtbus_ready = 0;
    pthread_mutex_unlock(&rtbus_task_mutex);
    // zlog()->debug("[RtBus] Get motion task signal");
}

TEST_F(PLANNERTEST, THREAD)
{
    planner_.init_traj_planner(PLAN_INTERVAL, TRAJ_INTERVAL);

    srand(time(0));

    // traj 线程 生产
    std::thread trajThread([this]() {
        pthread_setname_np(pthread_self(), "trajThread");
        EXPECT_EQ(planner_.get_planner_target().pos, 0);
        EXPECT_EQ(planner_.get_planner_target().is_planned, 0);
        for (int i = 0; i < DO_TIMES; i++)
        {
            rtbus_schedule_wait();
            usleep(4);
            TrajPoint a = point_list_.at(i);
            planner_.push_pos(a);  // 启动点，第一次push后，才是真正的插补，此时，直接以该点作为目标点；
            // printf("---push_pos %d\n", i);
        }
    });
    trajThread.detach();

    // rtbus 线程 消费
    std::thread rtbusThread([this]() {
        pthread_setname_np(pthread_self(), "rtbusThread");
        rtbus_schedule_motion();
        while (!planner_.get_tarj_start_flag()) { usleep(1000); }
        printf("rtbus start real interp!\n");

        TrajPoint pre = {};
        TrajPoint new_target = {};
        for (int i = 0; i < DO_TIMES * TRAJ_RATIO && !exit_flag; i++)
        {
            if (i % TRAJ_RATIO == 0)
            {
                TrajPoint last_last_target = planner_.get_planner_pre_target();
                TrajPoint last_target = planner_.get_planner_target();
                TrajPoint real_target = point_list_.at(i / TRAJ_RATIO);
                if (!last_target.is_planned)
                {
                    if (planner_.get_traj_lost_count() > 2)
                    {
                        printf("Seems period error!\n");
                        exit_flag = 1;
                        break;
                    }
                    ASSERT_NEAR(last_target.pos, real_target.pos, 1e-8);
                }
                else
                {
                    if (last_target.is_real && last_target.err == 0)
                    {
                        printf("loose point, %d!\n", i);
                        TrajPoint b = point_list_.at((i - 1) / TRAJ_RATIO);  // 丢点了，后面要预测
                        EXPECT_EQ(last_target.pos, b.pos);
                    }
                    else if (last_target.is_real && last_target.err != 0)  // 预测替换点，连续丢点了
                    {
                        ASSERT_NEAR(last_target.pos, (pre.pos + pre.vel * TRAJ_INTERVAL), 1e-8);
                    }
                    else
                    {
                        ASSERT_NEAR(last_target.pos, (pre.pos + pre.vel * TRAJ_INTERVAL), 1e-8);
                    }
                }

                planner_.do_interp(Planner::INTERP_LINEAR, Planner::PREDICT_HOLD_VEL);
                new_target = planner_.get_planner_target();
                EXPECT_EQ(new_target.is_planned, true);
                // printf("new target = %lld\n", new_target.pos);
                double interp_pos = 0;
                planner_.calc_s(TRAJ_RATIO * PLAN_INTERVAL, interp_pos);
                if (new_target.is_real)
                {
                    ASSERT_NEAR(real_target.pos, interp_pos, 1e-8);  // 期望到达目标点
                }
                else
                {
                    pre = planner_.get_planner_pre_target();
                    ASSERT_NEAR((pre.pos + pre.vel * TRAJ_INTERVAL), interp_pos, 1e-8);  // 期望到达插补点
                }
                printf("interp_pos %lf!\n", interp_pos);
                rtbus_schedule_motion();
                // printf("done!\n");
            }
            usleep(1);
        }
        rtbus_schedule_motion();
    });
    rtbusThread.join();
}